/*
 *  3D数学运算实现
 */
#ifndef _3D_MATH_H_
#define _3D_MATH_H_

#include <stdint.h>
#include <stdbool.h>

/*
 *  quaternion解算
 *  参数:
 *      quat_err[7]: 四元数和误差积累数组,初始值用 {1,0,0,0,0,0,0} (必要参数)
 *      valG[3]: 陀螺仪xyz轴输出,单位:deg/s (必要参数)
 *      valA[3]: 加速度xyz轴输出,单位:g  (可以置NULL,等于纯陀螺仪计算姿态)
 *      pry[3]: 输出绕xyz轴角度,单位:rad (可以置NULL)
 *      gravity[3]: 返回重力向量 (可以置NULL)
 *      intervalMs: 采样间隔,单位:ms (必要参数)
 */
void quat_pry(float quat_err[7], float valG[3], float valA[3], float pry[3], float gravity[3], int32_t intervalMs);

// 四元数角增量(龙格塔微分方程)
void quat_diff(float q[4], float roll_xyz[3]);
// roll_xyz使用单位: 度
void quat_diff2(float q[4], float roll_xyz[3]);

// 四元数乘法
void quat_multiply(float q1[4], float q2[4], float ret[4]);

// 欧拉角转四元数(zyx顺序)
void pry_to_quat(float pry[3], float q[4]);
// pry使用单位: 度
void pry_to_quat2(float pry[3], float q[4]);

// 四元数转欧拉角
void quat_to_pry(float q[4], float pry[3]);
// pry使用单位: 度
void quat_to_pry2(float q[4], float pry[3]);

/*
 *  四元数方式旋转和逆旋转
 *  参数:
 *      quat[4]: 使用已有的四元数(可置NULL), 将不使用 xyzSrc 和 rad
 *      xyzSrc[3]: 要绕转的空间向量,右手旋转,大拇指向量方向
 *      rad: 旋转角度,单位:rad
 *      xyzRet[3]: 被旋转的向量,输出结果覆写到此
 *      T: 转置
 */
void quat_roll(float quat[4], float xyzSrc[3], float rad, float xyzRet[3], bool T);
void quat_roll_array(float quat[4], float xyzSrc[3], float rad, float *xyzRet, int32_t len, bool T);

/*
 *  四元数依次三轴旋转
 *  参数:
 *      roll_xyz: 绕三轴旋转,单位:rad
 *      xyz: 目标点
 *      retXyz: 旋转和平移后结果写到此
 */
void quat_xyz(float roll_xyz[3], float xyz[3], float retXyz[3]);
void quat_zyx(float roll_xyz[3], float xyz[3], float retXyz[3]);


//获取平面三角形最长边
int32_t triangle_max_line(int32_t xy[6]);
int32_t triangle_max_line3D(int32_t xy[9]);

/*
 *  遍历平面三角形里面的每一个点
 *  参数:
 *      xy[6]: 3个二维坐标
 *      retXy: 返回二维坐标数组指针 !! 用完记得释放 !!
 *
 *  返回: retXy数组里的坐标个数
 */
int32_t triangle_enum(int32_t xy[6], int32_t **retXy);
int32_t triangle_enum3D(int32_t xyz[9], int32_t **retXyz); //三维版本

/*
 *  遍历平面直线所有的点
 *  参数:
 *      xy[4]: 2个二维坐标
 *      retXy: 返回二维坐标数组指针 !! 用完记得释放 !!
 *
 *  返回: retXy数组里的坐标个数
 */
int32_t line_enum(int32_t xy[4], int32_t **retXy);
int32_t line_enum3D(int32_t xyz[6], int32_t **retXyz); //三维版本

#endif // _3D_MATH_H_
